/*********************************************************************************
  *  @Copyright(C),1996-2021, Company
  *  @FileName: pid.c
  *  @Author: MO
  *  @Version: V1.0
  *  @Date: 2022-01-07 17:41:28
  *  @Description:
  *
  *
  *  @History:
       1.Date:
         Author:
         Modification:
       2.
**********************************************************************************/


/***************************************Includes***********************************/
#include "pid.h"


/************************************Private Macros********************************/
// #define


/**********************************Private Statement*******************************/


/***********************************Public Variables*******************************/



/**********************************Private Variables*******************************/



/**********************************Public Functions********************************/



/**********************************Private Functions*******************************/


/* [] END OF FILE */

/***************************************Includes***********************************/
#include "pid.h"


/***************************************Macros***********************************/
// #define


/***************************************Statement***********************************/
static float get_pid(pid_t *Instance, float input);
static void set_expect(pid_t *Instance, float expect);
static void set_pid(pid_t *Instance, float p, float i, float d);
static void set_output_limits(pid_t *Instance, float output_limits, float output_limits_min);
static void set_integral_limits(pid_t *Instance, float integral_limits_max, float integral_limits_min);

/***************************************Variables***********************************/


/***************************************Functions***********************************/

/**
 * @brief PID结构初始化
 * @param  Instance 结构句柄
 * @param  Kp
 * @param  Ki
 * @param  Kd
 * @param  expect   期望
 * @param  output_limits 输出极限
 * @param  integral_limits 积分极限
 * @date 2022-01-07 18:01:17
 * @note null
 */
void pid_init(pid_t *Instance, float Kp, float Ki, float Kd, float expect,
              float output_limits_max, float output_limits_min, float integral_limits_max, float integral_limits_min) {
    Instance->Kp = Kp;
    Instance->Ki = Ki;
    Instance->Kd = Kd;
    Instance->expect = expect;
    Instance->integral_sum = 0;
    Instance->last_input = 0;
    Instance->output_limits_max = output_limits_max;
    Instance->output_limits_min = output_limits_min;
    Instance->integral_limits_max = integral_limits_max;
    Instance->integral_limits_min = integral_limits_min;
    Instance->set_expect = set_expect;
    Instance->set_pid = set_pid;
    Instance->set_output_limits = set_output_limits;
    Instance->set_integral_limits = set_integral_limits;
    Instance->get_pid = get_pid;
}
/**
 * @brief 获取PID输出
 * @param  Instance
 * @param  input
 * @return float
 * @date 2022-01-07 18:01:91
 * @note null
 */
static float get_pid(pid_t *Instance, float input) {
    pid_t *p = Instance;
    float error = p->expect - input;
    float d_input = input - p->last_input;
    float Proportion = error * p->Kp;
    float differential = p->Kd * d_input;
    float output;
    p->last_input = input;
    p->integral_sum += p->Ki * error /* * 16*/;
    if (p->integral_sum > p->integral_limits_max)
        p->integral_sum = p->integral_limits_max;
    if (p->integral_sum < p->integral_limits_min)
        p->integral_sum = p->integral_limits_min;
    output = Proportion + p->integral_sum - differential;
    if (output > p->output_limits_max)
        output = p->output_limits_max;
    if (output < p->output_limits_min)
        output = p->output_limits_min;
    Instance->output = output;
    return output;
}
/**
 * @brief 设置期望
 * @param  Instance
 * @param  expect
 * @date 2022-01-07 18:01:71
 * @note null
 */
static void set_expect(pid_t *Instance, float expect) {
    Instance->expect = expect;
}
/**
 * @brief 设置PID系数
 * @param  Instance
 * @param  p
 * @param  i
 * @param  d
 * @date 2022-01-07 18:01:10
 * @note null
 */
static void set_pid(pid_t *Instance, float p, float i, float d) {
    Instance->Kp = p;
    Instance->Ki = i;
    Instance->Kd = d;
}
/**
 * @brief 设置输出极值
 * @param  Instance
 * @param  output_limits
 * @date 2022-01-07 18:01:53
 * @note null
 */
static void set_output_limits(pid_t *Instance, float output_limits_max, float output_limits_min) {
    Instance->output_limits_max = output_limits_max;
    Instance->output_limits_min = output_limits_min;
}
/**
 * @brief 设置积分极值
 * @param  Instance
 * @param  integral_limits
 * @date 2022-01-07 18:01:13
 * @note null
 */
static void set_integral_limits(pid_t *Instance, float integral_limits_max, float integral_limits_min) {
    Instance->integral_limits_max = integral_limits_max;
    Instance->integral_limits_min = integral_limits_min;
}



/* [] END OF FILE */
